//
// Created by admin on 2021/4/10.
// @author 梦小星
//
#ifndef AGV_DEFINES_H
#define AGV_DEFINES_H

//PID参数
#define kp 1
#define ki 1
#define kd 1
//1号电机端口定义
#define Motor1Pin1   32 //控制位3
#define Motor1Pin2   33 //控制位4
#define Motor1PwmPin    54  //使能调速A0
#define Motor1CountAPin 18 //编码器A 中断号：5

//2电机端口定义
#define Motor2Pin1   36 //控制位1
#define Motor2Pin2   37 //控制位2
#define Motor2PwmPin   56 //使能调速A2
#define Motor2CountAPin 20 //编码器A 中断号：3

//3号电机端口定义
#define Motor3Pin1   34 //控制位3
#define Motor3Pin2   35 //控制位4
#define Motor3PwmPin    55  //使能调速A1
#define Motor3CountAPin 19 //编码器A 中断号：4
//4电机端口定义
#define Motor4Pin1   38 //控制位1
#define Motor4Pin2   39 //控制位2
#define Motor4PwmPin    57  //使能调速A3
#define Motor4CountAPin 21 //编码器A 中断号：2
//5电机端口定义
#define Motor5Pin1   38 //控制位1
#define Motor5Pin2   39 //控制位2

//6电机端口定义
#define Motor6Pin1   38 //控制位1
#define Motor6Pin2   39 //控制位2
/*********************************************************
 * 函数功能：增量式PI控制器(左轮)
 * 入口参数：当前速度（编码器测量值），目标速度实际速度cm/s
 * 返回 值：电机PWM
 * 参考资料：
 *    增量式离散PID公式：
 *                Pwm-=Kp*[e(k)-e(k-1)]+Ki*e(k)+Kd*[e(k)-2e(k-1)+e(k-2)]
 *                e(k):本次偏差
 *                e(k-1):上一次偏差
 *                e(k-2):上上次偏差
 *                Pwm:代表增量输出
 *    在速度闭环控制系统里面我们只使用PI控制，因此对PID公式可简化为：
 *                Pwm-=Kp*[e(k)-e(k-1)]+Ki*e(k)
 *                e(k):本次偏差
 *                e(k-1):上一次偏差
 *                Pwm:代表增量输出
 *
 *    注意增量式PID先调I,再调P,最后再调D
 *********************************************************/
u8 Incremental_Pi(float current_speed, float target_speed);

/**************************************************************************
  函数功能：设置运动速度
  入口参数：pwm
  返回  值：无
**************************************************************************/
void Set_Pwm(int motor1PWM, int motor2PWM, int motor3PWM, int motor4PWM);

/**************************************************************************
 函数功能：编码器初始化
 入口参数：无
 返回  值：无
**************************************************************************/
void encoder_Init();

/***************************
 * 中断函数：读轮脉冲
 *
 **************************/
void addMotor1Count();


/**************************
 * 中断函数：读轮脉冲
 *
 *************************/
void addMotor2Count();

/***************************
 * 中断函数：读轮脉冲
 *
 **************************/
void addMotor3Count();


/**************************
 * 中断函数：读轮脉冲
 *
 *************************/
void addMotor4Count();


/***********************************
 * 电机实际速度计算：
 * 已知参数：
 *     车轮直径65mm,
 *     轮子一圈：131*11=1441个脉冲（RISING）
 * 单位时间读两个轮子脉冲读取两个轮子脉冲
 ***********************************/
void Read_Motor_V();

/**
 * 摇空器掉线重置数据
 *
 * */
void resetData();

/**
 * 运动控制函数
 * */
void moveForward();

void moveBackward();

void moveSidewaysRight();

void moveSidewaysLeft();

void rotateLeft();

void rotateRight();

void moveRightForward();

void moveRightBackward();

void moveLeftForward();

void moveLeftBackward();

void stopMoving();

/**
 * 调试函数，打印参数到串口监视器
 * */
void printData();

#endif //AGV_DEFINES_H
